#include "camControl/setRobot.hpp"

bool SetRobot::findRobotsPose2D(int __numRect, int* __colors,
        CvPoint* __centers) {

    if (this->ok) {

        this->allLocalizate = false;
        int numRobo = 0;
        INFO_ROBOT info;

        info.find = false;
        info.kalman = false;

        for (int i = 0; i < NUM_MAX_ROBOTS; i++)
            this->robot[i].setRobotInfo(info);

        for (int i = 0; i < __numRect - 1; i++) {
            for (int j = i + 1; j < __numRect; j++) {

                if ((__colors[i] == CYAN__ && __colors[j] == GREEN__) || (__colors[i] == GREEN__ && __colors[j] == CYAN__)) {

                    if (__colors[i] == CYAN__ && __colors[j] == GREEN__)
                        this->setPositionPixel(0, __centers[i], __centers[j]);
                    else
                        this->setPositionPixel(0, __centers[j], __centers[i]);
                    numRobo++;
                }

                if ((__colors[i] == CYAN__ && __colors[j] == PINK__) || (__colors[i] == PINK__ && __colors[j] == CYAN__)) {
                    if (__colors[i] == CYAN__ && __colors[j] == PINK__)
                        this->setPositionPixel(1, __centers[i], __centers[j]);
                    else
                        this->setPositionPixel(1, __centers[j], __centers[i]);
                    numRobo++;
                }
                if ((__colors[i] == CYAN__ && __colors[j] == ORANGE__) || (__colors[i] == ORANGE__ && __colors[j] == CYAN__)) {
                    if (__colors[i] == CYAN__ && __colors[j] == ORANGE__)
                        this->setPositionPixel(2, __centers[i], __centers[j]);
                    else
                        this->setPositionPixel(2, __centers[j], __centers[i]);
                    numRobo++;
                }
                if ((__colors[i] == PINK__ && __colors[j] == GREEN__) || (__colors[i] == GREEN__ && __colors[j] == PINK__)) {
                    if (__colors[i] == PINK__ && __colors[j] == GREEN__)
                        this->setPositionPixel(3, __centers[i], __centers[j]);
                    else
                        this->setPositionPixel(3, __centers[j], __centers[i]);
                    numRobo++;
                }
                if ((__colors[i] == PINK__ && __colors[j] == ORANGE__) || (__colors[i] == ORANGE__ && __colors[j] == PINK__)) {
                    if (__colors[i] == PINK__ && __colors[j] == ORANGE__)
                        this->setPositionPixel(4, __centers[i], __centers[j]);
                    else
                        this->setPositionPixel(4, __centers[j], __centers[i]);
                    numRobo++;
                }
                if ((__colors[i] == GREEN__ && __colors[j] == ORANGE__) || (__colors[i] == ORANGE__ && __colors[j] == GREEN__)) {
                    if (__colors[i] == GREEN__ && __colors[j] == ORANGE__)
                        this->setPositionPixel(5, __centers[i], __centers[j]);
                    else
                        this->setPositionPixel(5, __centers[j], __centers[i]);
                    numRobo++;
                }
            }
        }

        //        for (int i = 0; i < NUM_MAX_ROBOTS; i++)
        //            this->robot[i].updateKalman();

        if (numRobo == NUM_MAX_ROBOTS)
            this->allLocalizate = true;
    } else {
        return false;
    }
}

void SetRobot::image2world(int u, int v, double zpos,
        double *xpos, double *ypos) {

    double a = gsl_matrix_get(this->H, 0, 0);
    double b = gsl_matrix_get(this->H, 0, 1);
    double c = gsl_matrix_get(this->H, 0, 2) * zpos + gsl_matrix_get(this->H, 0, 3);
    double d = gsl_matrix_get(this->H, 1, 0);
    double e = gsl_matrix_get(this->H, 1, 1);
    double f = gsl_matrix_get(this->H, 1, 2) * zpos + gsl_matrix_get(this->H, 1, 3);
    double g = gsl_matrix_get(this->H, 2, 0);
    double h = gsl_matrix_get(this->H, 2, 1);
    double i = gsl_matrix_get(this->H, 2, 2) * zpos + gsl_matrix_get(this->H, 2, 3);

    *ypos = (c * d - v * c * g - u * d * i + u * f * g - a * f + v * a * i) / (a * e - u * e * g - v * a * h - b * d + u * d * h + v * b * g);
    *xpos = ((b - u * h) * (*ypos) + c - u * i) / (u * g - a);
}

void SetRobot::setPosition(int __numRobo, CvPoint __centers1, CvPoint __centers2) {

    double x1, x2, y1, y2;

    image2world(__centers1.x + this->nLeftoffset, __centers1.y + this->nTopoffset, Z_EPUCK, &x1, &y1);
    image2world(__centers2.x + this->nLeftoffset, __centers2.y + this->nTopoffset, Z_EPUCK, &x2, &y2);

    double dist = (x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2);
    INFO_ROBOT info;
    info.find = true;
    info.kalman = false;

    if (dist < DIST_THRESHOLD) {
        info.x = (x1 + x2) / 2;
        info.y = (y1 + y2) / 2;
        info.theta = atan2((y2 - y1), (x2 - x1));


        info.pixelX1 = __centers1.x;
        info.pixelX2 = __centers2.x;
        info.pixelY1 = __centers2.y;
        info.pixelY2 = __centers2.y;

        this->robot[__numRobo].setRobotInfo(info);
    }
}

void SetRobot::setPositionPixel(int __numRobo, CvPoint __centers1, CvPoint __centers2) {

    INFO_ROBOT info;

    int distX = __centers2.x - __centers1.x;
    int distY = __centers2.y - __centers1.y;

    distX *= distX;
    distY *= distY;

    int dist = distX + distY;

    if (dist < DIST_PIXEL) {

        info.find = true;

        info.pixelX1 = __centers1.x;
        info.pixelX2 = __centers2.x;
        info.pixelY1 = __centers1.y;
        info.pixelY2 = __centers2.y;

        //        printf("Antes 1: %d %d\n", info.pixelX1, info.pixelY1);
        //        printf("Antes 2: %d %d\n", info.pixelX2, info.pixelY2);

        info.pixelTheta1 = atan2(distY, distX);
        info.pixelTheta2 = atan2(distY, distX);

        this->robot[__numRobo].setRobotInfo(info);
    }
}

void SetRobot::printInformationSetRobo() {

    INFO_ROBOT info;
    for (int i = 0; i < NUM_MAX_ROBOTS; i++) {
        info = robot[i].getRobotInfo();
        printf("robo[%d]: \n", i);
        printf("[x, y, theta]: %f %f %f \n", info.x, info.y, info.theta);
        printf("[pixelX1, pixelY1, pixelTheta1]: %d %d %f\n", info.pixelX1, info.pixelY1, info.pixelTheta1);
        printf("[pixelX2, pixelY2, pixelTheta2]: %d %d %f\n", info.pixelX2, info.pixelY2, info.pixelTheta2);
        printf("[find, kalman]: %d %d \n", info.find, info.kalman);
    }
}

void SetRobot::setupKalmanFilterRobots() {

    this->ok = true;

    int DM = 8;
    int MM = 4;
    int CM = 0;

    double deltaTime = 0.005;
    double qX = 0.05;
    double qY = 0.05;
    //    double qTheta = 0.05;
    double rX = 0.1;
    double rY = 0.1;
    //    double rTheta = 0.1;

    double Q_[] = {
        qX, 0, 0, 0, 0, 0, 0, 0,
        0, qY, 0, 0, 0, 0, 0, 0,
        0, 0, qX, 0, 0, 0, 0, 0,
        0, 0, 0, qY, 0, 0, 0, 0,
        0, 0, 0, 0, qX, 0, 0, 0,
        0, 0, 0, 0, 0, qY, 0, 0,
        0, 0, 0, 0, 0, 0, qX, 0,
        0, 0, 0, 0, 0, 0, 0, qY
    };

    double R_[] = {
        rX, 0, 0, 0,
        0, rY, 0, 0,
        0, 0, rX, 0,
        0, 0, 0, rY
    };

    double A_[] = {
        1, 0, 0, 0, deltaTime, 0, 0, 0,
        0, 1, 0, 0, 0, deltaTime, 0, 0,
        0, 0, 1, 0, 0, 0, deltaTime, 0,
        0, 0, 0, 1, 0, 0, 0, deltaTime,
        0, 0, 0, 0, 1, 0, 0, 0,
        0, 0, 0, 0, 0, 1, 0, 0,
        0, 0, 0, 0, 0, 0, 1, 0,
        0, 0, 0, 0, 0, 0, 0, 1
    };

    double H_[] = {
        1, 0, 0, 0, 0, 0, 0, 0,
        0, 1, 0, 0, 0, 0, 0, 0,
        0, 0, 1, 0, 0, 0, 0, 0,
        0, 0, 0, 1, 0, 0, 0, 0
    };

    double p_[] = {
        1, 0, 0, 0, 0, 0, 0, 0,
        0, 1, 0, 0, 0, 0, 0, 0,
        0, 0, 1, 0, 0, 0, 0, 0,
        0, 0, 0, 1, 0, 0, 0, 0,
        0, 0, 0, 0, 1, 0, 0, 0,
        0, 0, 0, 0, 0, 1, 0, 0,
        0, 0, 0, 0, 0, 0, 1, 0,
        0, 0, 0, 0, 0, 0, 0, 1
    };

    double x_[] = {0, 0, 0, 0, 1, 1, 1, 1};

    for (int i = 0; i < NUM_MAX_ROBOTS; i++) {
        //        this->robot[i].setupKalmanFilter(DM, MM, CM, A_, NULL, H_, Q_, R_, p_, x_);
        this->robot[i].setupKalmanFilter();
    }
}
